#include <iostream>
#include <stdio.h>
#include "imu_track.h"
#include "complementary_filter.h"
#include "../utils/QueueUtil.h"



void print_hello_world()
{
    std::cout << "Hello, World!" << std::endl;
}


void test_imu_track()
{
    //1. 初始化
    // 1.1 初始化 complementary_filter结构体
    ComplementaryFilterV2 *filter = (ComplementaryFilterV2*)malloc(sizeof(ComplementaryFilterV2));
    initComplementaryFilter(filter);

    // 1.2 初始化时间戳
    double curr_time = 0.0;
    double prev_time = 0.0;
    double dt;

    // 1.3 初始化数据文件路径 和 结果保存文件路径 -- windows仿真环境
//    char logPath[] = "D:\\sensor_data.log";
//    char orientLogPath[] = "D:\\orientation_result.log";
    char logPath[] = "D:\\sensor_data.log";
    char orientLogPath[] = "D:\\orientation_result.log";
    FILE* fpImuLog, *fpPoseLog;
    ImuLogLine imuData;
    // if (fopen_s(&fpImuLog, logPath, "r") != 0) {
    //     printf("Couldn't open the file.");
    //     return;
    // }

    // if (fopen_s(&fpPoseLog, orientLogPath, "w") != 0) {
    //     printf("Error opening file.\n");
    //     return;
    // }

    // //2. 循环读取文件，获取imu数值,执行本次计算，结果写入文件
    // while (fscanf(fpImuLog, "%f %f %f %f %f %f %f %f", &imuData.timeStamp, &imuData.AccX, &imuData.AccY, &imuData.AccZ, &imuData.GyroX,
    //                 &imuData.GyroY, &imuData.GyroZ) != EOF) {

    //     // 计算deltaT
    //     curr_time = imuData.timeStamp;
    //     if(prev_time == 0.0) {
    //         //initial
    //         dt = 0.005; // imu频率200Hz,故另第一个deltaT = -1/200
    //     } else {
    //         dt = (curr_time - prev_time) / (double)1000;
    //     }
    //     prev_time = curr_time;

    //     //计算每次方向
    //     update(filter, imuData.AccX, imuData.AccY, imuData.AccZ, imuData.GyroX, imuData.GyroX, imuData.GyroX, dt);

    //     //记录每步计算结果
    //     fprintf(fpPoseLog, "%f\t%f\t%f\t%f\t%f\n",
    //             filter->q->q0, filter->q->q1, filter->q->q2, filter->q->q3, filter->time);
    // }

}






